import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import Command, LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterValue

def generate_launch_description():
    # 获取URDF文件路径
    urdf_package_path = get_package_share_directory('robot_description')
    default_urdf_path = os.path.join(urdf_package_path, 'urdf','robot','robot.urdf.xacro')
    default_rviz_config_path = os.path.join(urdf_package_path,'config','display_robot.rviz')

    # 声明URDF参数
    declare_urdf_model_path = DeclareLaunchArgument(
        name="model",
        default_value=str(default_urdf_path),
        description="路径参数：加载URDF模型文件"
    )

    # 从URDF路径中读取文件内容
    robot_description_value = ParameterValue(
        Command(['xacro ', LaunchConfiguration('model')]),
        value_type=str
    )

    # 定义robot_state_publisher节点
    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"robot_description": robot_description_value}],
        output="screen"
    )

    # 定义joint_state_publisher节点
    joint_state_publisher_node = Node(
        package="joint_state_publisher",
        executable="joint_state_publisher",
        output="screen"
    )

    # 定义RViz2节点
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        arguments=['-d',default_rviz_config_path],
        output="screen"
    )

    # 返回LaunchDescription对象
    return LaunchDescription([
        declare_urdf_model_path,
        robot_state_publisher_node,
        joint_state_publisher_node,
        rviz_node
    ])
